/*
 *  Physics.h
 *  c_Render
 *
 *  Created by liao peterson on 11/28/09.
 *  Copyright 2009 uestc. All rights reserved.
 *
 */
#ifndef PHYSICS_H__
#define PHYSICS_H__
#include "Macros.h"
#include "Mathutilis.h"
#include "DataStructures.h"
#include "GameMeshes.h"
#include "Audio.h"

const GLfloat DURATION = 0.01f;
const GLfloat GRAVITY_CONSTANT = 150.0f;
const GLfloat STATIC_MASS = 1.0f;
const GLfloat AIR_RESTITUITION = 0.98f;
const GLfloat CONTACT_BOUNCE = 0.2f;
const GLint	  SLEEPING_CHECK = 10;				// Period for checking sleeping
const GLfloat SLEEPING_CHANGE = 0.005f;
const GLfloat DEPARTURE_DIST = 0.1f;			//Departure distance for contact data update.
const GLfloat CONTACT_RESTITUITION = 20.0f;
const GLint	  MAX_POSITION_ITERATIONS = 50;				// For multi-contact resolution.
const GLint   MAX_VELOCITY_ITERATIONS = 50;		// For multi-contact resolution.
const GLfloat PENETRATION_SHRINKAGE = 0.9f;	// For fixing position over adujsting in applyPosChange
const GLfloat INITIAL_INERTIA = 100.0f;
const GLint	  MAX_CONTACT_LIFE = 5;

const vec3 Z_UP = {0,0,1};
const vec3 ZERO_VEC = {0,0,0};

enum _BoundType{
	BOX,
	SPHERE,
	STATIC,
	UNKNOWN
};

typedef struct _PHYSICS_INFO{
	_BoundType	boundType;
	box			*bBox;						//Bounding Box Data
	GLfloat		bRadius;					//Sphere Bounding Radius.
	GLfloat		mass;						//-1 for infinite mass
	vec3		loc;						//Dynamic location
	vec3		vel;						//velocity
	vec3		acc;						//acceleration
	vec3		locOffset;					//location offset
	vec3		axis;						//rotation axis
	GLfloat		theta;						//current rotation angle
	vec3		angVel;						//angular velocity
	vec3		angAcc;						//angular acceleration
	Quat		q;
	GLfloat		uniMoment;
	Matrix3x3	moment;						//moment of inertia
}physicsInfo;

class force{
public:
	vec3 f;
	vec3 offset;					//offset from the object's center of mass(world coordinate)
									//Haaaaas to have a limit,right?
									//Does it track the object or not?
	GLboolean changed;
	
	force(){changed=true;f.x=0;f.y=0;f.z=0;offset.x=0;offset.y=0;offset.z=0;}
	force(const vec3&nf){f=nf;changed=true;}
	force(const vec3&nf, const vec3&no){f = nf;offset = no;changed=true;}
	virtual void updateForce(physicsInfo&);
};

class gravity : public force{
public:
	void updateForce(physicsInfo&);
};

typedef force impulse;			//They're nearly the same

//////////////////////Physical Delegate for object
class physicsIntegrator{
public:
	List<force> *forceRegistry;
	Mesh		*target;
	physicsInfo	pi;
	GLboolean	moving;				//moving?
	GLboolean   rotating;			//rotating?
	GLboolean	fChanged;			//force changed?
	GLboolean	accelerating;		//loc changed?still accelerating?
	GLboolean	angAccelerating;				//angular acceleration?
	Matrix4x4	transform;			//include the Model to World transform for objects, for getAxis function
	Matrix4x4	itransform;			//the inverse transform of above.
	GLint		identity;			// physics integrators' unique identifier, used for contact data.
	static GLint	currentID;		//used for naming physics integrator
	
	physicsIntegrator();
	~physicsIntegrator();
	void update();
	void updateTorque();
	void applyForce(force*target);
	void applyImpulse(impulse*target);
	void applyImpulse(impulse*target, vec3&velChange,vec3&angVelChange);
	//Matrix4x4 inverseTransorm();
	void setMatrix();
	
	void updateVel();
	void updatePos();
	
	vec3 getAxis(GLint) const;
	
	////////////////////////////////ATTENTION!
	//Very dirty variable!
	GLint deleted;
	
};

//////////////////////Collision code goes here
class contact{
public:
	/**
	 * Holds the position of the contact in world coordinates.
	 */
	vec3 contactPoint;
	// Local contact point aka the most original pos without any transform.
	// Two of them for contact data update.
	vec3 locContactPointOne;
	vec3 locContactPointTwo;
	
	/**
	 *Holds the direction of the contact in the world coordinates.
	 *Pointing from one to two.
	 */
	vec3 contactNormal;
	
	/**
	 * Holds the depth of penetration at the contact point. If both
	 * bodies are specified then the contact point should be midway
	 * between the inter-penetrating points.
	 * <0 when interpenetrating.
	 */
	GLfloat penetration;
	// Used for deciding departure
	GLfloat estimatedPenetration;
	GLfloat originalPenetration;
	
	GLfloat restitution;
	
	// Indices for best axis on physicsIntegrator one and two.
	// Possible value:1,2,3
	GLint indexOne;
	GLint indexTwo;
	
	physicsIntegrator*one;
	physicsIntegrator*two;
	
	// Contact velocity in contact coordinate
	// contact2World.Transpose * (One's speed - Two's speed)
	// Its x is positive when colliding
	vec3 contactVel;
	Matrix3x3 contact2World;
	
	// Resolving variabls
	GLfloat deltaVelocity;
	GLfloat desiredDeltaVelocity;
	vec3 relativeContactPosition[2];
	// For resolution rotation use.
	GLfloat rotRes[2];
	vec3 velChange[2];
	vec3 angVelChange[2];
	vec3 posChange[2];
	vec3 oriChange[2];
	
	GLint lifeCycle;
	
	contact();
	GLboolean Compare(contact*data);
	void copyVecs(contact*data);
	void recalPen();
	void SetContact(vec3 cPoint,vec3 cNormal,GLfloat pen,GLint index1,GLint index2,physicsIntegrator*A,physicsIntegrator*B);
	vec3 getAxis();
	/*
	 *Below Funcs For resolution usage
	 */
	// Calculating internal data: deltaVelocity and desiredDeltaVelocity
	void calInternal();
	void calContactTransform();
	GLfloat calDeltaVel(int index);
	void calDesiredDeltaVel();
};

typedef struct _COLLISION_DATA{
	List<contact>	*contacts;
	_COLLISION_DATA(){contacts = new List<contact>;}
	~_COLLISION_DATA(){delete contacts;}
}collisionData;


class collisionManager{
public:
	bool Sphere2Sphere(physicsIntegrator&, physicsIntegrator&, collisionData*);
	GLint Box2Box(physicsIntegrator&, physicsIntegrator&, collisionData*) const;
	bool Box2Point(physicsIntegrator&, physicsIntegrator&, const vec3&, contact*) const;
	bool Box2Point(physicsIntegrator&target, const vec3&point) const;
	bool Line2Box(const line&, const physicsIntegrator&, GLfloat&tMin);
	inline bool SlabTest(const line&, const plane&, const GLfloat, GLfloat&, GLfloat&);
	// Contact normal pointing from one to two.
	void collisionResolveVel(collisionData*);
	void collisionResolvePos(collisionData*);
	
	void updateCache(collisionData*);
	collisionData*cache;
	
	collisionManager(){cache = new collisionData;};
	~collisionManager(){delete cache;}
};

class physicsSimulator{
public:
	//line finger;
	//GLushort fingerFlag;

	List<physicsIntegrator> *fList;
	collisionManager * colManager;
	Audio*sound;
	//generalAlgorithm * algorithm;
	
	physicsSimulator(){fList = new List<physicsIntegrator>;
						colManager = new collisionManager;
						sound = new Audio;
						//sound->initOpenAL();
					  }
	//physicsSimulator(vec3 point, vec3 dir){fList = new List<physicsIntegrator>; finger = {dir, point};}
	~physicsSimulator(){delete fList; sound->teardownOpenAL();}
	void update();
	GLint updateFinger(line&l);
};
#endif